from moveitJpoint import go
import math
from pickle import LONG1
import numpy
from math import sin,cos
import rospy

def joint1(y,x):
    return math.degrees(math.atan(y/x))

def sqrt(a,b):
    return math.sqrt(a**2 + b**2)

def L2A(a,b,c):
        
    cosA = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)
    cosB = (a ** 2 + c ** 2 - b ** 2) / (2 * a * c)
    cosC = (a ** 2 + b ** 2 - c ** 2) / (2 * a * b)

    angleA = math.degrees(math.acos(cosA))
    angleB = math.degrees(math.acos(cosB))
    angleC = math.degrees(math.acos(cosC))

    return angleA,angleB,angleC


def MoveTo(point,MoveFlag = False):
    L1 = 0.333
    L2 = 0.316
    L3 = 0.384
    LendFront = 0.088
    LendHeight = 0.107 + 0.12
    E3 = 0.0825
    E2 = 0.0825
    L2F = sqrt(L2,E2)
    L3F = sqrt(L3,E3)

    
    x,y,z = point

    if sqrt(x,y) < 0.4:
        print("Too close to chassis,Now:",sqrt(x,y))
        return None

    j1 = joint1(y,x)

    ############
    if z + LendHeight > L1:
        dh = (z + LendHeight) - L1 
    else:
        dh = L1 - ( z + LendHeight )
    r = sqrt(x,y) - LendFront
    dist = sqrt(r,dh) 


    aL2F,aL3F,adist1 = L2A(L2F,L3F,dist)
    adh,ar,adist2 = L2A(dh,r,dist)
    aE3,aL3,alL3F = L2A(E3,L3,L3F)
    aE2,aL2,alL2F = L2A(E2,L2,L2F)


    if z + LendHeight > L1:
        j1 = joint1(y,x)
        j2 = 90 - adh - aL3F - aE2
        a = (360 - aL2 - aL3 - adist1)
        j4 = 180 - (180 - (360 - aL2 - aL3 - adist1))
        j7 = ar + aL2F + aE3
    else:
        j1 = joint1(y,x)
        j2 = 180 - (aE2 + aL3F + ar)
        a = 360 - aL2 - aL3 - adist1
        j4 = 180 - (180 - (a))
        j7 = 90 + (adh + aL2F + aE3)

    print("joint1:",j1)
    print("joint2:",j2)
    print("joint4:",-1 * j4)
    print("joint7:",j7)
    
    if MoveFlag == True:
        go(j1,j2,-1 * j4,j7)

def Home():
    go(0,-27,-81,78)

if __name__ == "__main__":
    import cv2
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
    Home()
    cv2.waitKey(1000)
    MoveTo(point = [0.56336228,  0.18664352,  0.24484259],MoveFlag=True)
    cv2.waitKey(1000)
    